/*
    This file is part of ciberRatoToolsSrc.

    Copyright (C) 2001-2006 Universidade de Aveiro

    ciberRatoToolsSrc is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    ciberRatoToolsSrc is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Foobar; if not, write to the Free Software
    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

import java.io.*;
import java.net.*;
import java.util.*;
import java.util.Vector;

import ciberIF.*;


/**
 * example of a basic agent
 * implemented using the java interface library.
 */






public class final1 {

     public ciberIF cif;
    public int i=0,j=0;
	//For Localization
    public double PrevLeftOut ;
    public double PrevRightOut ;
    public double PresLeftIn;
    public double PresRightIn;
    public double CurDir;
    public double CurX;
    public double CurY;	
    public BeaconData beacondata; 
    public int BeaconFoundat0 ;
    public double BeaconFinalAngleat0;
    public double FinalTargetX;
    public double FinalTargetY; 
    public int BeaconFoundUntilNow;
    public double lpow;
    public double rpow;
    public int LeftWallTracing;
    public int RightWallTracing;
    public int UpdateCount;
	public int ontarget;
      
 //Keller's code
   public MouseMap mousemap;
   public boolean bumper;
   public int walltracelength;
   
//wall tracing 
   public int DirectionTurned;
   public double Angle_Truned_At_Wall;
    public double DirectionChangeTime;

	public static void main(String[] args) {

	String host, robName;
	int pos; 
	int arg;
         
	//default values
	host = "localhost";
	robName = "final1";
	pos = 1;


        // parse command-line arguments
	try {
	    arg = 0;
	    while (arg<args.length) {
		if(args[arg].equals("-pos")) {
			if(args.length > arg+1) {
				pos = Integer.valueOf(args[arg+1]).intValue();
				arg += 2;
			}
		}
		else if(args[arg].equals("-robname")) {
			if(args.length > arg+1) {
				robName = args[arg+1];
				arg += 2;
			}
		}
		else if(args[arg].equals("-host")) {
			if(args.length > arg+1) {
				host = args[arg+1];
				arg += 2;
			}
		}
		else throw new Exception();
	    }
	}
	catch (Exception e) {
		print_usage();
		return;
	}
	
	// create client
	final1 client = new final1();

	// register robot in simulator
	client.cif.InitRobot(robName, pos, host);
   
        //wait till the simulator starts
	client.cif.ReadSensors();
	while(client.cif.GetTime() == 0){client.cif.ReadSensors(); }
   //simulator started        

         
        // main loop
	client.mainLoop();

		
//Return Home starts
   //startret

	System.out.println("IAM Starting to return");
	client.startret();
	}

    // Constructor
  public   final1() {
	    cif = new ciberIF();
	    beacon = new beaconMeasure();
            beacondata = new BeaconData();
         
            BeaconFoundat0 = 0 ;    
	    beaconToFollow = 0;
            PrevLeftOut = 0.0;
	    PrevRightOut = 0.0; 
            PresLeftIn=0.0;
            PresRightIn=0.0;
	    CurX=0.0;
	    CurY=0.0; 
	    CurDir=0.0; 
            lpow=0;
            rpow=0;
            FinalTargetX = 20.0;
            FinalTargetY = 0.5;  
            LeftWallTracing=RightWallTracing = -1; BeaconFoundUntilNow=0;
            UpdateCount =0;
            DirectionTurned=-1;
	    DirectionChangeTime=0;
            walltracelength=0;
			ontarget = 0;

             
    }

 public void UpdateCoordinates(double lpow,double rpow)
{             
         
        double lin,rot; 
	//System.out.println("Previous Outpurs : Left = "+PrevLeftOut + "Right = "+PrevRightOut);
        PrevLeftOut =  (PrevLeftOut + lpow)/2;
	PrevRightOut = (PrevRightOut + rpow)/2;
     //   System.out.println("Present Outpurs : Left = "+PrevLeftOut + "Right = "+PrevRightOut);
        lin = (PrevLeftOut +PrevRightOut)/2;
	rot = (PrevRightOut - PrevLeftOut);	
	CurX = CurX + lin*Math.cos(mousemap.getCompass());
	CurY = CurY + lin*Math.sin(mousemap.getCompass());
        CurDir += rot;
        if(mousemap.getCompass() > Math.PI) CurDir -= (2.0 * Math.PI);
	else if(mousemap.getCompass() < -Math.PI) CurDir += (2.0 * Math.PI);
        System.out.println("\n The Current Coordinates are X : "+ CurX +" Y : "+CurY+ "angle = " + CurDir*180/Math.PI+ " count = "+UpdateCount++ + "lpow ="+lpow + "rpow = "+rpow );  


}


    /** 
     * reads a new message, decides what to do and sends action to simulator
     */
    public void mainLoop () {
	
        int i= 0;
        double BeaconFoundCompassAngle=0;
	double BeaconFoundatAngle=0;
        double ToGoBeaconNotFound=0;
        double Farthest_Reading=100; 
	double compass_tmp=0;
        int BeaconAngleFound = 0;
        
	beaconMeasure beacon_tmp = new beaconMeasure(); 
        double InitialOrientation=0; 
         while(i<10)
	{  UpdateCount++;
                cif.RequestCompassSensor();  
		cif.ReadSensors();
              if(cif.IsCompassReady()){
              InitialOrientation = cif.GetCompassSensor(); }
           i++;
	}
        if(Math.abs(InitialOrientation) < 20 )
         {
           InitialOrientation = 0;
         }
        else if (Math.abs(InitialOrientation) > 75)
        {
           if( InitialOrientation > 75) InitialOrientation = 90;
           else InitialOrientation = -90;
        }
        else {
             if(InitialOrientation > 150) InitialOrientation = 180;
             else if(InitialOrientation < -150) InitialOrientation = -180;   
         }
        System.out.println("Initial Orientation = "+InitialOrientation);
//keller
         CurDir = InitialOrientation*Math.PI/180 ;
         mousemap = new MouseMap(CurDir);

//	Turn 360, try and find the beacon. 
	i=0;
	while (i <25)
	{ 
            double irSensor0,irSenosor1,irSensor2;
            irSensor0=irSensor1=irSensor2=-1;
	 if(i%2 == 0)
         { 
		if(i%4 == 0)
		{
			cif.RequestIRSensor(0);
			cif.RequestIRSensor(2);
			
		}
           else { cif.RequestIRSensor(0);
                 cif.RequestIRSensor(1); }   
	 }
         else { cif.RequestCompassSensor();
		cif.RequestBeaconSensor(beaconToFollow); }
 //keller
        mousemap.update_loc(rpow,lpow,bumper);
	
        UpdateCoordinates(lpow,rpow);
        cif.ReadSensors();
        //System.out.println(" GPS angle ="+ cif.GetDir()+"  X="+cif.GetX()+ "Y = "+cif.GetY());    
        if(cif.IsBumperReady()){ bumper = cif.GetBumperSensor();}
	if(cif.IsCompassReady()) {
	compass_tmp = cif.GetCompassSensor();  
	System.out.println("Compass Reading = "  + compass_tmp  ); }
	if(cif.IsBeaconReady(0)) {
        beacon_tmp = cif.GetBeaconSensor(0);
	System.out.println("Beacon Reading = " + beacon_tmp.beaconDir +" Cur Direction"+ (CurDir*180/Math.PI) ); }
	if(cif.IsObstacleReady(1)) { irSensor1= cif.GetObstacleSensor(1);}
        if(cif.IsObstacleReady(2)) { irSensor2 = cif.GetObstacleSensor(2);}    
	if(cif.IsObstacleReady(0)) {
                irSensor0 = cif.GetObstacleSensor(0);                    
		if(Farthest_Reading> irSensor0)
		{ Farthest_Reading = irSensor0; ToGoBeaconNotFound = CurDir*180/Math.PI;}    
                                  }
//keller
        if(irSensor1 > -1){mousemap.update_board(irSensor1,-1,irSensor0,bumper);}
        if(irSensor2 > -1) {mousemap.update_board(-1,irSensor2,irSensor0,bumper); } 

        cif.DriveMotors(0.125,-0.125);
        lpow= 0.125; rpow = -0.125; 
	
	if( (i>8) && (beacon_tmp.beaconDir != 0) )
	{
                BeaconFoundUntilNow=1;  
		BeaconFoundat0 = 1;
		BeaconFoundCompassAngle = compass_tmp;
		BeaconFoundatAngle = beacon_tmp.beaconDir;
                BeaconFinalAngleat0 = BeaconFoundCompassAngle + BeaconFoundatAngle;
		System.out.println("BEACON FOUND updating the target angle: " +BeaconFinalAngleat0 + "CurDir:"+CurDir*180/Math.PI);
		
	}
	++i;
	}
         
       double TargetAngle;
       if(BeaconFoundat0 == 1){
	//beacondata.BeaconUpdate(CurX,CurY,BeaconFoundatAngle,BeaconFoundCompassAngle);
        TargetAngle = BeaconFinalAngleat0 ;}
       else TargetAngle = ToGoBeaconNotFound;
       double CurDirDegrees = CurDir*180/Math.PI;
        System.out.println("Finally TargetAngle and Curdir"+TargetAngle +":" + CurDir*180/Math.PI);
       while( !((Math.toDegrees(mousemap.getCompass())-TargetAngle) <15 &&(Math.toDegrees(mousemap.getCompass()) -TargetAngle) > -15) )
       { 
         UpdateCoordinates(lpow,rpow);
	 mousemap.update_loc(rpow, lpow, bumper);        
         cif.ReadSensors();
         //System.out.println("    GPS angle ="+ cif.GetDir()+"  X="+cif.GetX()+ "Y = "+cif.GetY()); 
         cif.DriveMotors(0.125,-0.125);
         lpow=0.125; rpow= -0.125;
         System.out.println("Aligning the robot to the target directoin. " + CurDir*180/Math.PI +"target =" + TargetAngle);
       }
 
     
               
//TODO : Check if the above angle should be CurDir or PrevDir.
           
        cif.DriveMotors(0.125,0.125);
        UpdateCoordinates(0.125,0.125);
        mousemap.update_loc(0.125,0.125,bumper);  


	
        while(true) {   //Rest of the main loop
       
        if(BeaconFoundUntilNow == 0){
        if (cif.GetTime()%200 ==0){ SearchBeacon(); System.out.println("calling SearchBeacon");   }        
	System.out.println("increasing the wall tracelength");
        
         
                 }    
       		
			cif.ReadSensors();
			if(cif.GetTime() == 600) return;
             //System.out.println("    GPS angle ="+ cif.GetDir()+"  X="+cif.GetX()+ "Y = "+cif.GetY());
		decide();
		
		UpdateCoordinates(lpow, rpow);
		//keller
		mousemap.update_loc(rpow, lpow, bumper);  


	}
    }

	//Return function 

	public void startret()
	{
		/*cif.SetReturningLed(true);
		int[] x ={ 1, -2, -1, -2, 2, 3, 10, 12, 11 };
		int[] y ={ -2, 2, -3, 4, -3, 5, 4, 4, 2 };*/
		ArrayList path = new ArrayList();
		path=mousemap.get_path_home(2);
		int i = 0;
		System.out.println("the path home is of size= " + path.size());
		for (; i < path.size(); i++)
		{
			pair target;
			
			//find the angle to which we should turn the Robot
			double dist;
			//cif.SetReturningLed(false);
			double slope = 0.0;
			System.out.println("the angle current is " + mousemap.getCompass());
			double x, y;
			
			int stop=0;
			double time=0;
			//slope=(target1.y-target.y)/(target1.x-target.x);
			int orient=0;


			while (true)
			{
				target = (pair)path.get(i);
				x = target.getX();
				y = target.getY();

				if ((x - mousemap.getXpos())!= 0)
					slope = (y - mousemap.getYpos()) / (x - mousemap.getXpos());
				else
				{
					System.out.println("THE POINT IS x:" + x + "y:" + y + "Cur x:" + mousemap.getXpos() + "Cur y:" + mousemap.getYpos());
					System.exit(0);
				}
				System.out.println("The slope is " + slope);
				double oeta = Math.atan(slope);
				if (mousemap.getXpos() > x && mousemap.getYpos() > y)
				{
					System.out.println("IAM HERE HAI");
					oeta = -1 * (Math.PI - oeta);
				}
				if (mousemap.getXpos() > x && mousemap.getYpos() < y)
				{
					System.out.println("IAM IN SECOND");
					oeta = Math.PI - Math.abs(oeta);
				}

				System.out.println("The angle is " + oeta);
				int clock;//clockwise or anti clockwise orientation so as to decide power to be given
				if ((oeta >= (Math.PI - Math.abs(mousemap.getCompass())) && oeta <= Math.PI) || (oeta < mousemap.getCompass()))
					clock = 1;//turn clockwise
				else
					clock = 0;//turn anti clockwise
				System.out.println("The clock value is " + clock);

				dist = Math.sqrt(Math.pow((x - mousemap.getXpos()), 2) + Math.pow((y - mousemap.getYpos()), 2));
				System.out.println("The distance to travel is " + dist);
				cif.ReadSensors();

				//sensing for other robots
				boolean f;
				f=cif.GetBumperSensor();

				if (cif.IsObstacleReady(0))
				{
					System.out.print(" 0 READY	");
					irSensor0 = cif.GetObstacleSensor(0);
				}
				if (cif.IsObstacleReady(1))
				{
					System.out.print(" 1 READY 	");
					irSensor1 = cif.GetObstacleSensor(1);
				}
				if (cif.IsObstacleReady(2))
				{
					System.out.print(" 2 READY	");
					irSensor2 = cif.GetObstacleSensor(2);
				}
				if (stop > 0 && cif.GetTime() >= time + 20)
				{
					cif.DriveMotors(0, 0);
					rpow = 0;
					lpow = 0;
					mousemap.update_loc(rpow, lpow, f);
					path = mousemap.get_path_home(0);
					stop = 0;
					i = 0;

				}
				else if (irSensor0 > 4.0 || irSensor1 > 4.0 || irSensor2 > 4.0)
				{
					System.out.println("CHANGING DIRECTION");
					cif.DriveMotors(0.1, -0.01);
					rpow = -0.01;
					lpow = 0.1;
					mousemap.update_loc(rpow, lpow, f);
					if (stop < 6) time = cif.GetTime();
					stop ++;
					
				}

				else if (irSensor1 > 1.0)
				{
					cif.DriveMotors(0.05, 0.0);
					rpow = 0.0;
					lpow = 0.05;
					mousemap.update_loc(rpow,lpow,f);
					if(stop<6)time = cif.GetTime();
					stop ++;
				}

				else if (irSensor2 > 1.0)
				{
					cif.DriveMotors(0.0, 0.05);
					rpow = 0.05;
					lpow = 0.0;
					mousemap.update_loc(rpow,lpow,f);
					if (stop < 6) time = cif.GetTime();
					stop ++;
				}
				else if (stop < 0)
				{
					cif.DriveMotors(.05, .05);
					rpow = 0.05;
					lpow = 0.05;
					mousemap.update_loc(rpow, lpow, f);
				}




				else
				{
					if (dist >= 0.9)
					{
						if (orient != 1 && Math.abs(oeta - mousemap.getCompass()) >= 0.3)
						{
							if (clock == 0)
							{
								System.out.println("Turn anticlockwise");
								cif.DriveMotors(0.0, 0.1);
								rpow = 0.1;
								lpow = 0.0;
								mousemap.update_loc(rpow, lpow, f);
							}
							else
							{
								System.out.println("IAM TURNING CLOCKWISE");
								cif.DriveMotors(0.1, 0.0);
								rpow = 0.0;
								lpow = 0.1;
								mousemap.update_loc(rpow, lpow, f);
							}
						}
						else
						{
							orient = 1;
							System.out.println("The cur direction is" + mousemap.getCompass());
							System.out.println("IAM ORIENTED IN THE DIRECTION");
							System.out.println("Iam moving in the direction");
							cif.DriveMotors(0.1, 0.1);
							rpow = 0.1;
							lpow = 0.1;
							mousemap.update_loc(rpow, lpow, f);
						}
					}
					else
					{
						
						System.out.println("Iam at the Point");
						System.out.println("x:" + mousemap.getXpos() + "y:" + mousemap.getYpos());
						if (i == (path.size() - 1))
						{
							cif.DriveMotors(0, 0);
							mousemap.update_loc(0, 0, f);

						}
						else
							mousemap.update_loc(rpow, lpow, f);
						break;
					}
				}
				System.out.println("The time is " + cif.GetTime());
				if (cif.GetTime() % 2 == 0)
				{
					cif.RequestIRSensor(0);
					cif.RequestIRSensor(1);
				}
				else
				{
					cif.RequestIRSensor(0);
					cif.RequestIRSensor(2);
				}
			}
		}
	}

    /**
     * basic reactive decision algorithm, decides action based on current sensor values
     */
    
	public void decide() {

 	 
      if(cif.IsBumperReady()){ bumper = cif.GetBumperSensor(); }
     
               irSensor0=irSensor1=irSensor2=-1;
 



            if(cif.IsObstacleReady(0)){
		   irSensor0 = cif.GetObstacleSensor(0); }
	    if(cif.IsObstacleReady(1)){
		irSensor1 = cif.GetObstacleSensor(1);}
	    if(cif.IsObstacleReady(2)){
		irSensor2 = cif.GetObstacleSensor(2);}
	    if(cif.IsCompassReady()) { 
		    compass = cif.GetCompassSensor(); }
	    if(cif.IsGroundReady()) {  
		    ground = cif.GetGroundSensor(); } 
	    if(cif.IsBeaconReady(beaconToFollow)) { 
		    beacon = cif.GetBeaconSensor(beaconToFollow);
                     
                    beacondata.BeaconUpdate(mousemap.getXpos(),mousemap.getYpos(),-1000,mousemap.getCompass(),(int)(cif.GetTime()+9));
                     }
//CHANGED PRAVEEN
            for(int p=0; p< beacondata.BeaconDataSize; p++)
		{    
         //         System.out.println("BeaconDataSize = "+ beacondata.BeaconDataSize +"Iterator = "+p);
                   if(beacondata.locations[p].UpdateTime == cif.GetTime()) {

                      beacon = cif.GetBeaconSensor(beaconToFollow);
                      
                      beacondata.locations[p].BeaconAngle = beacon.beaconDir;
                      if(beacon.beaconDir != 0) BeaconFoundUntilNow=1; 
        System.out.println("Beacon For Locations X = "+beacondata.locations[p].X+" Y="+beacondata.locations[p].Y+" BeaconAngle="+beacondata.locations[p].BeaconAngle+" CurrAngle="+beacondata.locations[p].CurrAngle);
                      }
                }
//CHANGED PRAVEEN
              if(irSensor0 > -1){ mousemap.update_board(-1,-1,irSensor0,bumper); }
              else if(irSensor1 > -1) {mousemap.update_board(irSensor1,irSensor2,-1,bumper); }
            System.out.println( "Measures: ir0=" + irSensor0 + " ir1=" + irSensor1 + "ir2=" + irSensor2 + "compass angle:"+ compass+ "beacon angle:"+ beacon.beaconDir + "\n");
            System.out.println("The present time = " + cif.GetTime()+ "\n");	    
	
	

	if(irSensor0 >4.0 || irSensor1 >4.0 ||  irSensor2 >4.0) { 
//CHANGED PRAVEEN
         System.out.println("\n Really close");
	 if(irSensor1>4.0) { cif.DriveMotors(0.125,-0.1); lpow = 0.125; rpow = -0.1;
                              if((BeaconFoundUntilNow == 1) &&  (DirectionTurned == -1))
                                  DirectionTurned = 0; 
                                  DirectionChangeTime = cif.GetTime(); 
                           }
         else if(irSensor2>4.0) { if(BeaconFoundUntilNow == 0){
                                  	if((LeftWallTracing == -1) && (RightWallTracing == -1))
                                     	{ RightWallTracing = 1; 
                                        System.out.println("\n Beacon NOT FOUND Right Wall Tracing"); } 
                                   }                              
                                  cif.DriveMotors(0.125,-0.1); lpow = 0.125; rpow = -0.1;
 				  if((BeaconFoundUntilNow==1) &&  (DirectionTurned == -1))
                                      {DirectionTurned = 0;
					DirectionChangeTime = cif.GetTime(); }
                                 }
	  else{         if(BeaconFoundUntilNow == 0){
                                  	if((LeftWallTracing == -1) && (RightWallTracing == -1))
                                     	{ LeftWallTracing =1; 
                                           System.out.println("\n Beacon NOT FOUND Left Wall Tracing");  } 
                                   } 
                        cif.DriveMotors(0.125,-0.125); lpow = 0.125; rpow = -0.125;
            		if((BeaconFoundUntilNow==1) &&  (DirectionTurned == -1)){
                                  DirectionTurned = 0;
				  DirectionChangeTime = cif.GetTime(); }
              }
//CHANGED PRAVEEN		
   
		
	} 
        else if((irSensor1!=-1)&&(irSensor1<1.0) && (LeftWallTracing == 1)&&(BeaconFoundUntilNow == 0)) {cif.DriveMotors(0.0,0.1); 
                                                             System.out.println("pushing to left wall");lpow = 0.0; rpow = 0.1;}
        else if((irSensor2!=-1)&&(irSensor2<1.0) && (RightWallTracing == 1)&&(BeaconFoundUntilNow == 0)) {cif.DriveMotors(0.1,0.0);
                                                               System.out.println("pushing to right wall"); lpow = 0.1; rpow =0.0; } 

	else if(irSensor1>1.0) { cif.DriveMotors(0.1,0.0); lpow= 0.1; rpow= 0.0; 
                  
   		System.out.println("irSensor1:"+irSensor1 +"  LEFT MOTOR: "+ i++ +"\n" );
		}
	else if(irSensor2>1.0)  { cif.DriveMotors(0.0,0.1); lpow = 0.0; rpow = 0.1;
                               
 		System.out.println("irSensor2:"+irSensor2 + "   RIGHT MOTOR: "+ j++ + "\n"); 
		}
        
	else if(beacon.beaconVisible && beacon.beaconDir > 20.0) { 
		System.out.println("Beacon LESS than 20 \n"); 
		cif.DriveMotors(0.0,0.05); lpow = 0.0; rpow = 0.05; //UpdateCoordinates(lpow,rpow);

		}
        else if(beacon.beaconVisible && beacon.beaconDir < -20.0) {  
		System.out.println("Beacon GREATER than 20");
		cif.DriveMotors(0.05,0.0); lpow = 0.05; rpow = 0.0 ; //UpdateCoordinates(lpow,rpow);

		}
        else  { //No Significant sensor reading
                 System.out.println("No significant Data BeaconFound="+BeaconFoundUntilNow);
                   	
                   {    
 
              		if(BeaconFoundUntilNow == 0){
             		   cif.DriveMotors(0.1,0.1); lpow = 0.1; rpow = 0.1;}
                	else {
                  		if(beacondata.Target_Location_Available == 1) {  
                 
                  		double CurrentSlope; 
                  
  
                 		if(FinalTargetX - mousemap.getXpos() !=0 ) 
						{  //make sure tan inverse doesnt end up in a problem
                       		CurrentSlope = Math.atan( (FinalTargetY-CurY)/(FinalTargetX-mousemap.getXpos()) );


                 		if(mousemap.getXpos()>FinalTargetX && CurY>FinalTargetY)
				{
				CurrentSlope=-1*(Math.PI-CurrentSlope);     }
				if(mousemap.getXpos()>FinalTargetX && CurY< FinalTargetY)
				{
				CurrentSlope=Math.PI-Math.abs(CurrentSlope);  }
				int clock;
				if ((CurrentSlope >= (Math.PI - Math.abs(mousemap.getCompass())) && CurrentSlope <= Math.PI) || (oeta < mousemap.getCompass()))
					clock = 1;
				else
					clock = 0;
				double dist;
				dist = Math.sqrt(Math.pow((FinalTargetX - mousemap.getXpos()), 2) + Math.pow((FinalTargetY - mousemap.getYpos()), 2));
				if (dist >= 0.9)
				{
					if (Math.abs(CurrentSlope - mousemap.getCompass()) >= 0.3)
					{
						if (clock == 0)
						{
							cif.DriveMotors(-0.1, 0.1);
							lpow = -0.1;
							rpow = 0.1;
						}
						else
						{
							cif.DriveMotors(0.1, -0.1);
							lpow = 0.1;
							rpow = -0.1;
						}
					}


							 /*if(CurrentSlope < CurDir) {
								if(CurDir-CurrentSlope > 0.2) {cif.DriveMotors(0.1,0.0); lpow=0.1; rpow=0.0; } else { cif.DriveMotors(0.125,0.125); lpow=0.125; rpow=0.125;}            System.out.println("\nMoving Right to target");
								   } 
							 else if(CurrentSlope > CurDir) { 
							if(CurrentSlope-CurDir > 0.2) {cif.DriveMotors(0.0,0.1); lpow= 0.0; rpow=0.1;} else {cif.DriveMotors(0.125,0.125); lpow=0.125; rpow=0.125;}  System.out.println("\nMoving Left to target"); 
									   }*/
					else
					{
						cif.DriveMotors(0.125, 0.125);
						lpow = 0.125; rpow = 0.125;
						System.out.println("\nMoving Straight to Target");
					}
				}
				else
				{
					ontarget = 1;
					if (cif.IsGroundReady())
					{
						if (ground == beaconToFollow) {
							cif.SetReturningLed(true);							 
						 }  
					  }

				}

                } //if(FinalTargetX - mousemap.getXpos() !=0 ) 
							                     
                   
                                                      } //beacon.Target_location_avail=1
                                 
                                 else{ System.out.println("\n GOING STRAIGHT");cif.DriveMotors(0.1,0.1); lpow = 0.1 ; rpow = 0.1;}
                           }

                     
                    
                     }//DirectionTruned !=-1

                     
                 } //End of no significant sensor 


				 if (ontarget == 0)
				 {
					 if (cif.GetTime() % 2 == 0)
					 {
						 cif.RequestIRSensor(0);

						 if (cif.GetTime() % 8 == 0)
						 {
							 cif.RequestGroundSensor();
						 }

						 else
						 {
							 cif.RequestBeaconSensor(beaconToFollow);
						 }
					 }
					 else
					 {
						 cif.RequestIRSensor(1);
						 cif.RequestIRSensor(2);

					 }
				 }
				 else
				 {

					 if (cif.GetTime() % 2 == 0)
					 {
						 cif.RequestIRSensor(0);

						 cif.RequestGroundSensor();
					 }
					 else
					 {
						 cif.RequestIRSensor(1);
						 cif.RequestIRSensor(2);
					 }
				 }

    }

    static void print_usage() {
             System.out.println("Usage: java final1 [-robname <robname>] [-pos <pos>] [-host <hostname>[:<port>]]");
    }

//SEARCH BEACON FUNCTION



public void SearchBeacon()
{
int i = 0;
double BeaconFinalAngle=0;
double lpow=0,rpow=0;
double compass_tmp=0;
boolean bumper=false;
beaconMeasure beacon_tmp = new beaconMeasure();
double BeaconFoundCompassAngle=0,BeaconFoundatAngle=0;

System.out.println("Searching the Beacon");
 while (i <25)
	{ 
            double irSensor0,irSenosor1,irSensor2;
            irSensor0=irSensor1=irSensor2=-1;
	 if(i%2 == 0)
         { 
		if(i%4 == 0)
		{
			cif.RequestIRSensor(0);
			cif.RequestIRSensor(2);
			
		}
           else { cif.RequestIRSensor(0);
                 cif.RequestIRSensor(1); }   
	 }
         else { cif.RequestCompassSensor();
		cif.RequestBeaconSensor(beaconToFollow); }
 
        if(i!=0)mousemap.update_loc(rpow,lpow,bumper);
//call mouse update board
	
        if(i!=0)UpdateCoordinates(lpow,rpow);
        cif.ReadSensors();

        if(cif.IsBumperReady()){ bumper = cif.GetBumperSensor();}
	if(cif.IsCompassReady()) {
	compass_tmp = cif.GetCompassSensor();  
	System.out.println("Compass Reading = "  + compass_tmp  ); }
	if(cif.IsBeaconReady(0)) {
        beacon_tmp = cif.GetBeaconSensor(0);
	System.out.println("Beacon Reading = " + beacon_tmp.beaconDir +" Cur Direction"+ (CurDir*180/Math.PI) ); }
	if(cif.IsObstacleReady(1)) { irSensor1= cif.GetObstacleSensor(1);}
        if(cif.IsObstacleReady(2)) { irSensor2 = cif.GetObstacleSensor(2);}    
	if(cif.IsObstacleReady(0)) {
                irSensor0 = cif.GetObstacleSensor(0);                    
		   
                                  }
//keller
        if(irSensor1 > -1){mousemap.update_board(irSensor1,-1,irSensor0,bumper);}
        if(irSensor2 > -1) {mousemap.update_board(-1,irSensor2,irSensor0,bumper); } 

        cif.DriveMotors(0.125,-0.125);
        lpow= 0.125; rpow = -0.125; 
	
	if( (i>8) && (beacon_tmp.beaconDir != 0) )
	{
                BeaconFoundUntilNow=1;  
		BeaconFoundCompassAngle = compass_tmp;
		BeaconFoundatAngle = beacon_tmp.beaconDir;
                BeaconFinalAngle = BeaconFoundCompassAngle + BeaconFoundatAngle;
		break;
		
	}
	++i;
	}
         
       double TargetAngle;
       if(BeaconFoundUntilNow == 1){
        TargetAngle = BeaconFinalAngle ;
     
       while( !((Math.toDegrees(mousemap.getCompass())-TargetAngle) <15 &&(Math.toDegrees(mousemap.getCompass()) -TargetAngle) > -15) )
       { 
         UpdateCoordinates(lpow,rpow);
	 mousemap.update_loc(rpow, lpow, bumper);        
         cif.ReadSensors();
         //System.out.println("    GPS angle ="+ cif.GetDir()+"  X="+cif.GetX()+ "Y = "+cif.GetY()); 
         cif.DriveMotors(0.125,-0.125);
         lpow=0.125; rpow= -0.125;
         System.out.println("Aligning the robot to the target directoin. " + Math.toDegrees(mousemap.getCompass())+"target =" + TargetAngle);
       }
        }
     
               
//TODO : Check if the above angle should be CurDir or PrevDir.
           
        cif.DriveMotors(0.125,0.125);
        UpdateCoordinates(0.125,0.125);
        mousemap.update_loc(0.125,0.125,bumper);  

  
}
//SEARCH BEACON FUNCTION



    private double irSensor0, irSensor1, irSensor2, irSensor3,  compass;
    private beaconMeasure beacon;
    private int    ground;
    private boolean collision;

    private int beaconToFollow;
    
    

};








class BeaconData
{ 
//No of locations where beacon is found
public int No_Found;
public int Target_Location_Available;  
public double TargetX;
public double TargetY;

//point beacon is found first time
public double Found1X ;     
public double Found1Y ;
public double Found1Angle;
//point beacon is found second time
public double Found2X ;
public double Found2Y ;
public double Found2Angle;
//Last nine locations where BeconReadings were taken
public BeaconLocation[] locations;
//repesents the number of BeaconUpdates received
public int BeaconDataSize;  

public BeaconData()
{
 No_Found = 0;
 Target_Location_Available = 0; 
 BeaconDataSize = 0; 
locations = new BeaconLocation[9];
}

public void AddLocation(double X, double Y, double Angle)
{
 System.out.println("AddLocation :: beacon at X ="+X +" Y="+Y+" Angle= "+Angle);
 
if(Angle >30){  
    if(No_Found == 0)
    { 
      this.Found1X= X;
      this.Found1Y = Y ;
      this.Found1Angle = Angle;
      No_Found++;
      
    }
    else if(No_Found == 1)
    {	
	this.Found2X = X;
        this.Found2Y = Y;
        this.Found2Angle = Angle;
       No_Found++;
    }
    else
    {
        this.Found1X=this.Found2X;
        this.Found1Y=this.Found2Y; 
	this.Found1Angle=this.Found2Angle;
	this.Found2X = X;
        this.Found2Y = Y;
        this.Found2Angle = Angle;
        No_Found++;
    } 

	if(No_Found>1) //implies the becon is found in more than 2 locations. So we calculate the target location. 
	{
          if(Found1Angle<0) Found1Angle=Found1Angle+180;
          if(Found2Angle<0) Found2Angle=Found2Angle+180;
          double numerator,denominator;
           numerator = (Found2Y-Found1Y + Math.tan(Math.toRadians(Found1Angle))*Found1X - Math.tan(Math.toRadians(Found2Angle))*Found2X);
           denominator =      (Math.tan(Math.toRadians(Found1Angle)) - Math.tan(Math.toRadians(Found2Angle)) );
          TargetX = numerator/denominator;
          //System.out.println("Target X Numerator == "+numerator+ " denominator=="+denominator);
          numerator = ( Found2X-Found1X + (Found1Y/Math.tan(Math.toRadians(Found1Angle))  - Found2Y/Math.tan(Math.toRadians(Found2Angle))) );
          denominator = ( 1/Math.tan(Math.toRadians(Found1Angle)) - 1/Math.tan(Math.toRadians(Found2Angle)) );
          TargetY = numerator/denominator;
         //System.out.println("Target Y Numerator == "+numerator+ " denominator=="+denominator);
       System.out.println("BeaconTarget Calculated from X1= "+ Found1X +" X2 ="+ Found2X + " Y1=" +Found1Y+" Y2="+Found2Y+" angle1="+Found1Angle+" angle2"+Found2Angle);    
       System.out.println("Beacon Target Ready at  X=" + TargetX +"   Y="+ TargetY);     
  	Target_Location_Available = 1;
    
		
	}
}


} 
public void BeaconUpdate(double X,double Y,double BeaconAngle,double CurrAngle,int UpdateTime)
{  
   System.out.println("\n BeaconUpdate X = "+X +" Y="+Y+" BeaconAngle="+BeaconAngle+" CurrAngle="+CurrAngle); 
   locations[BeaconDataSize] = new BeaconLocation(X,Y,BeaconAngle,CurrAngle,UpdateTime);
    BeaconDataSize++ ;
     	for(int i=0;i<BeaconDataSize; i++)  
     	{ 
        System.out.println("Iterating Locations X = "+locations[i].X+" Y="+locations[i].Y+" BeaconAngle="+locations[i].BeaconAngle+" CurrAngle="+locations[i].CurrAngle);
         if( locations[i].BeaconAngle != -1000 ) 
         { if(locations[i].BeaconAngle != 0) {AddLocation(locations[i].X,locations[i].Y,locations[i].BeaconAngle+locations[i].CurrAngle);}
          //  System.out.println("Removing one of the locations");
           for(int j=i;j<BeaconDataSize-1; j++) { locations[j]=locations[j+1]; }
           BeaconDataSize--;
         }
                        
     	}
      	
   	
}


} //end of BeaconData class

class BeaconLocation {
public double X;
public double Y;
public double BeaconAngle;
public double CurrAngle;
public int UpdateTime;


public BeaconLocation(double X,double Y,double BeaconAngle, double CurrAngle,int UpdateTime) 
{
  this.X = X;
  this.Y = Y;
  this.BeaconAngle = BeaconAngle;
  this.CurrAngle = CurrAngle; 
  this.UpdateTime = UpdateTime;
}


}//end of BeaconLocation

